﻿using System;
using System.Drawing;
using System.IO;
using System.Net.Sockets;
using System.Threading;
using com.atiia.automation.sensors;

namespace SenRobCSTest
{
    public class FTsensors
    {
        private static string masterIPDefault = "146.232.146.253";
        private static string slaveIPDefault = "146.232.146.254";

        private static string barionetIP2 = "barionet023.bing.sun.ac.za";
        private static int barionetPort2 = 12301;

        private static int pcUDPport = 5151;

        private double[] m_daftMaxes = { 100, 100, 100, 100, 100, 100 };    // maximum rated ft readings
        private double[] m_daftCountsPerUnit = { 1, 1, 1, 1, 1, 1 };        // counts per unit ft for each axis

        private string m_strForceUnits;
        private string m_strTorqueUnits;
        private string m_strCalSN;
        private string m_strCalIndex;
        private string m_strCfgName;
        private string m_strCfgIndex;

        private string m_strSensorAddress1;
        private string m_strSensorAddress2;

        private static int NUM_FT_AXES = 6;
        private static Color POSITIVE_COLOR = Color.Blue, NEGATIVE_COLOR = Color.Green;

        // net ft sensor api stuff
        private NetFTSensor m_netFT1;
        private NetFTReaderThread m_NetFTReaderThread1 = null;
        private Socket m_cNetFTDataSocket;

        private NetFTSensor m_netFT2;
        private NetFTReaderThread m_NetFTReaderThread2 = null;

        bool m_bDoingHighSpeedConnection = false; // states whether or not we're doing high speed data collection
        System.IO.StreamWriter m_cDataWriter; // writes high speed data to file

        HapticControl RobotControl = new HapticControl();
        UDPcommand CommUDP = new UDPcommand();
        string udpR = "";

        string Robcmnd = "";
        string RobcmndTime = "";
        string RobcmndUDP = "";
        int cc = 0;
        string SendOK = "OK";
        int SendOKCheck = 0;

        MainForm m_Parent;

        public FTsensors(MainForm set_Parent)
        {
            this.m_Parent = set_Parent;
            m_strSensorAddress1 = Microsoft.VisualBasic.Interaction.InputBox("Prompt", "Please enter master IP", masterIPDefault, -1, -1);
            m_strSensorAddress2 = Microsoft.VisualBasic.Interaction.InputBox("Prompt", "Please enter slave IP", slaveIPDefault, -1, -1);

            stopAndRestartSingleReadings();
        }

        // relay robot updates to correct robot
        public void UpdateRobot(NetFTRDTPacket RDT, int sensorID)
        {
            if (sensorID == 1)
                robotControl.UpdateSensor1(RDT);
            else if (sensorID == 2)
                robotControl.UpdateSensor2(RDT);
        }

        class NetFTReaderThread
        {
            private long UdpTimeCount;
            private long UdpTimeWait = DateTime.Now.Ticks;

            private NetFTSensor m_netFT; // net FT controller
            private bool m_bKeepGoing = true; // ends reading

            private FTsensors m_Parent;

            private int sensorID = -1;

            System.Threading.Thread thisThread;

            // create new Net FT reader thread
            public NetFTReaderThread(NetFTSensor setNetFT, FTsensors setParent, int sensorID)
            {
                m_netFT = setNetFT;
                m_Parent = setParent;
                this.sensorID = sensorID;
            }

            // set flag to stop after current read is complete
            public void stopReading()
            {
                m_bKeepGoing = false;
                thisThread.Interrupt(); // wake up thread if it is currently sleeping
            }

            public void runThread()
            {
                NetFTRDTPacket cRDTData; // latest FT data from Net FT

                while (m_bKeepGoing)
                {
                    try
                    {
                        if (m_Parent.m_bDoingHighSpeedConnection)
                        {
                            // read batch RDT packets
                            int PACKETS_TO_READ = 1000;
                            NetFTRDTPacket[] caRDTPackets = new NetFTRDTPacket[PACKETS_TO_READ];
                            caRDTPackets = m_netFT.readHighSpeedData(m_Parent.m_cNetFTDataSocket, PACKETS_TO_READ);

                            int i;
                            /* Precondition: caRDTPackets contains the list of packets that were read from the Net FT
                             * m_cDataWriter is open to file to collect data to.
                             * Postcondition: m_cDataWRiter has the new FT data.
                             * i = PACKETS_TO_READ
                             */
                            for (i = 0; i < PACKETS_TO_READ; i++)
                            {
                                m_Parent.m_cDataWriter.WriteLine(dataCollectionLine(caRDTPackets[i]));
                            }
                            cRDTData = caRDTPackets[PACKETS_TO_READ - 1];
                        }
                        else
                            cRDTData = m_netFT.readSingleFTRecord();

                        m_Parent.UpdateRobot(cRDTData, this.sensorID);
                        m_Parent.displayFTData(cRDTData, this.sensorID);

                        m_Parent.Robcmnd = m_Parent.RobotControl.Inc_Control_Robot();

                        if (m_Parent.Robcmnd.CompareTo("sensor data not valid") != 0)
                        {
                            m_Parent.RobcmndTime = m_Parent.Robcmnd.Substring(0, 3);
                            m_Parent.RobcmndUDP = m_Parent.Robcmnd.Substring(3);
                            UdpTimeCount = DateTime.Now.Ticks;

                            if (m_Parent.m_Parent.getUDPtoggle().Checked && this.sensorID == 2 && SendOKCheck == 1)
                            {
                                m_Parent.SendOK = m_Parent.CommUDP.UDPReceive(pcUDPport);
                            }
                            if (m_Parent.m_Parent.getUDPtoggle().Checked && this.sensorID == 1)
                            {
                                m_Parent.displayError2("SendOK: " + m_Parent.SendOK);

                                if (m_Parent.SendOK.Equals("OK"))
                                {
                                    m_Parent.displayError1("cmnd: " + m_Parent.Robcmnd);
                                    UdpTimeWait = DateTime.Now.Ticks;
                                    m_Parent.SendOK = "";
                                    m_Parent.CommUDP.UDPSend(m_Parent.BarionetIP2, m_Parent.BarionetPort2, m_Parent.RobcmndUDP);
                                    m_Parent.SendOKCheck = 1;
                                }
                            }
                        }
                    }
                    catch (SocketException sexc)
                    {
                        m_Parent.displayError1("Socket Exception: " + sexc.getMessage());
                    }
                    catch (IOException iexc)
                    {
                        m_Parent.displayError1("IO Exception: " + iexc.getMessage());
                    }

                    try
                    {
                        if (!m_Parent.m_bDoingHighSpeedCollection)
                        {
                            Thread.Sleep(100);
                        }
                    }
                    catch (Exception iexc)
                    {
                        /*do nothing, just continue.  This exception should
                         only be thrown if they try to stop the thread while
                         it's sleeping*/
                    }
                }
            }
        }

        /**Formats a data packet for collection to file.
        * @param nftrdtp   The data packet to record to file.
        * @return  A formatted string containing the F/T data and current time,
        * which can be written to file.
        */

        private String dataCollectionLine(NetFTRDTPacket nftrdtp)
        {
            String outputString =  /* The formatted output line. */
                    "0x" + Convert.ToString(nftrdtp.getStatus(), 16) + "," +
                    nftrdtp.getRDTSequence() + "," +
                    nftrdtp.getFTSequence() + "," +
                    nftrdtp.getFx() + "," +
                    nftrdtp.getFy() + "," +
                    nftrdtp.getFz() + "," +
                    nftrdtp.getTx() + "," +
                    nftrdtp.getTy() + "," +
                    nftrdtp.getTz() + "," +
                    DateTime.Now.Ticks.ToString();

            return outputString;
        }

        /**
* Stops the F/T reader thread, and sets m_NetFTReaderThread1 =
* null.
*/

        private void stopReaderThread()
        {
            if (null != m_NetFTReaderThread1)
            {
                m_NetFTReaderThread1.stopReading();
                m_NetFTReaderThread1 = null;
            }
            if (null != m_NetFTReaderThread2)
            {
                m_NetFTReaderThread2.stopReading();
                m_NetFTReaderThread2 = null;
            }
        }

        /**Stops (if necessary) the net f/t reader thread, and restarts it using
         *the user options. Also sets the scaling factors for the f/t display.
         */

        private void stopAndRestartSingleReadings()
        {
            stopAndRestartSingleReadings(m_netFT1, m_NetFTReaderThread1, m_strSensorAddress1, 1);
            stopAndRestartSingleReadings(m_netFT2, m_NetFTReaderThread2, m_strSensorAddress2, 2);
        }

        private void stopAndRestartSingleReadings(NetFTSensor m_netFT_common, NetFTReaderThread m_NetFTReaderThread_common, String m_strSensorAddress_common, int sensorID)
        {
            if (null != m_NetFTReaderThread_common)
            {
                m_NetFTReaderThread_common.stopReading();
                m_NetFTReaderThread_common = null;
            }
            try
            {
                m_netFT_common = new NetFTSensor(System.Net.IPEndPoint.IPAddress.Parse(m_strSensorAddress_common)); // possible problem
            }
            catch (UnknownHostException uhex)
            {
                displayError1("Unknown Host Exception: " +

                                    uhex.getMessage());
                return;
            }
            if (!readConfigurationInfo())
            {
                return;
            }

            //lblForceUnits.setText( "Force Units: " + m_strForceUnits );
            //lblTorqueUnits.setText( "Torque Units: " + m_strTorqueUnits );
            //lblCalIndex.setText("Calibration Index: " + (Integer.parseInt(m_strCalIndex) + 1));
            //lblConfigIndex.setText("Config Index: " + (Integer.parseInt(m_strCfgIndex) + 1));
            //lblCalSN.setText("Calibration Serial#: " + m_strCalSN);
            //lblConfigName.setText("Config Name: " + m_strCfgName);
            m_NetFTReaderThread_common = new NetFTReaderThread(m_netFT_common, this, sensorID);
            m_NetFTReaderThread_common.Start();
        }

        ///**
        // *Reads a page from the integrated web server.
        // *@param strUrlSuffix   The page on the web server to read.
        // *@return  The text of the web page.
        // *@throws MalformedURLException If strUrlSuffix doesn't point to a valid
        // *web page address.
        // *@throws IOException If there is an error reading the web page text.
        // */

        //private String readWebPageText(String strUrlSuffix)
        //{
        //    /*Reads the HTML from the web server.*/
        //    BufferedReader cBufferedReader;
        //    /*The url of the configuration page.*/
        //    String strURL = "http://" + m_strSensorAddress1 + "/" +
        //            strUrlSuffix;

        //    cBufferedReader = new BufferedReader(new InputStreamReader(new

        //            URL(strURL).openConnection().getInputStream()));

        //    /*The text of the page.*/
        //    String strPageText = "";
        //    /*The last line read from the web stream.*/
        //    String strCurLine;
        //    /*Precondition: cBufferedReader is at the beginning of the page.
        //     *Postcondition: cBufferedReader is finished, strPageText =
        //     *the text of the page, strCurLine = last line read from the
        //     *page.
        //     */
        //    while (null != (strCurLine = cBufferedReader.readLine()))
        //    {
        //        strPageText += strCurLine;
        //    }

        //    return strPageText;
        //}

        //private String readNetFTAPI(int index)
        //{
        //    try
        //    {
        //        String strXML = readWebPageText("netftapi2.xml?index=" + index);
        //        return strXML;
        //    }
        //    catch (Exception e)
        //    {
        //        return "";
        //    }
        //}

        //private String readNetFTCalAPI(int index)
        //{
        //    try
        //    {
        //        String strXML = readWebPageText("netftcalapi.xml?index=" + index);
        //        return strXML;
        //    }
        //    catch (Exception e)
        //    {
        //        return "";
        //    }
        //}

        //private int findActiveCFG(String xmlText)
        //{
        //    String[] strret = xmlText.split("<setcfgsel>");

        //    String[] strret2 = strret[1].split("</setcfgsel>");

        //    int activeConfig = Integer.parseInt(strret2[0]);

        //    return activeConfig;
        //}

        ///**
        // *Reads information about the sensor's configuration from the integrated
        // *web server.
        // *@return  True if configuration was succesfully read, false otherwise.
        // */

        //private boolean readConfigurationInfo()
        //{
        //    try
        //    {
        //        String mDoc = readNetFTAPI(0);
        //        int activeConfig = findActiveCFG(mDoc);
        //        mDoc = readNetFTAPI(activeConfig);
        //        m_strCfgIndex = "" + activeConfig;
        //        String[] parseStep1 = mDoc.split("<cfgcalsel>");
        //        String[] parseStep2 = parseStep1[1].split("</cfgcalsel>");
        //        String mCal = readNetFTCalAPI(Integer.parseInt(parseStep2[0]));
        //        m_strCalIndex = parseStep2[0];
        //        parseStep1 = mCal.split("<calsn>");
        //        parseStep2 = parseStep1[1].split("</calsn>");
        //        m_strCalSN = parseStep2[0];
        //        mDoc = readNetFTAPI(activeConfig);
        //        parseStep1 = mDoc.split("<cfgnam>");
        //        parseStep2 = parseStep1[1].split("</cfgnam>");
        //        m_strCfgName = parseStep2[0];
        //        parseStep1 = mDoc.split("<cfgcpf>");
        //        parseStep2 = parseStep1[1].split("</cfgcpf>");
        //        setCountsPerForce(Double.parseDouble(parseStep2[0]));
        //        parseStep1 = mDoc.split("<cfgcpt>");
        //        parseStep2 = parseStep1[1].split("</cfgcpt>");
        //        setCountsPerTorque(Double.parseDouble(parseStep2[0]));
        //        parseStep1 = mDoc.split("<comrdtrate>");
        //        parseStep2 = parseStep1[1].split("</comrdtrate>");
        //        m_iRDTSampleRate = (Integer.parseInt(parseStep2[0]));
        //        parseStep1 = mDoc.split("<scfgfu>");
        //        parseStep2 = parseStep1[1].split("</scfgfu>");
        //        m_strForceUnits = parseStep2[0];
        //        parseStep1 = mDoc.split("<scfgtu>");
        //        parseStep2 = parseStep1[1].split("</scfgtu>");
        //        m_strTorqueUnits = parseStep2[0];
        //        parseStep1 = mDoc.split("<cfgmr>");
        //        parseStep2 = parseStep1[1].split("</cfgmr>");
        //        String[] asRatings = parseStep2[0].split(";");
        //        for (int i = 0; i < asRatings.length; i++)
        //        {
        //            m_daftMaxes[i] = Double.parseDouble(asRatings[i]);
        //            if (0 == m_daftMaxes[i])
        //            {
        //                m_daftMaxes[i] = 32768; /* Default maximum rating. */
        //            }
        //        }
        //        //m_ftvc.setMaxForce(m_daftMaxes[2]); /* Use Fz rating as maximum. */
        //        //m_ftvc.setMaxTorque(m_daftMaxes[5]); /* use Tz rating as maximum. */
        //    }
        //    catch (Exception e)
        //    {
        //        return false;
        //    }
        //    return true;
        //}

        /**
         *Sets the maximum ratings for each axis.
         *@param strConfigPageText  The HTML code of the configuration page on the
         *integrated web server.
         */

        private void setCountsPerForce(double counts)
        {
            double dCountsPerForce = counts;
            if (0 == dCountsPerForce)
            {
                dCountsPerForce = 1;
                displayError1("Read a counts per force value of 0, setting it to " +
                        "1 instead.");
            }
            int i;
            for (i = 0; i < 3; i++)
            {
                m_daftCountsPerUnit[i] = dCountsPerForce;
            }
        }

        private void setCountsPerTorque(double counts)
        {
            double dCountsPerTorque = counts;
            if (0 == dCountsPerTorque)
            {
                dCountsPerTorque = 1;
                displayError1("Read a counts per torque value of 0, setting it " +
                        "to 1 instead.");
            }
            int i; /*generic loop/array index.*/
            /*Precondition: dCountsPerForce has the counts per force,
             *dCountsPerTorque is the counts per torque.
             *Postcondition: m_daftCountsPerUnit has the counts per unit for each
             *axis, i == 3.
             */
            for (i = 0; i < 3; i++)
            {
                m_daftCountsPerUnit[i + 3] = dCountsPerTorque;
            }
        }

        public void Form_Closing()
        {
            stopReaderThread();
            try
            {
                if (m_bDoingHighSpeedCollection)
                {
                    m_netFT1.stopDataCollection(m_cNetFTDataSocket);
                    m_netFT2.stopDataCollection(m_cNetFTDataSocket);
                }
            }
            catch (IOException ioexc)
            {
                JOptionPane.showMessageDialog(this, "An IOException has " +
                        "occurred: " + ioexc.getMessage(), "IOException",
                        JOptionPane.ERROR_MESSAGE);
            }
        }

        /**Runnable which updates the Force and Torque display using data
        *from an RDT packet.  Used exclusively by the DisplayFTData method.
        */

        private class UpdateFTDisplay
        {
            private NetFTRDTPacket m_rdtPacket;
            private int sensorID = -1;
            /**Creates a new UpdateFTDisplay
             *@param setRDTPacket   The NetFTRDTPacket which contains the
             *F/T data to display.
             */

            public UpdateFTDisplay(NetFTRDTPacket setRDTPacket, int sensorID)
            {
                m_rdtPacket = setRDTPacket;
                this.sensorID = sensorID;
            }

            private void update1()
            {
             status1.Text = ( "0x" + String.Format(  "%08x", (int)m_rdtPacket.getStatus() ) );
            lblRDTSeq1.setText( "" + m_rdtPacket.getRDTSequence() );
            lblFTSeq1.setText( "" + m_rdtPacket.getFTSequence() );
            int ftCountsReading[] = m_rdtPacket.getFTArray();
            double ftRealReading[] = new double[NUM_FT_AXES];
            int i;

            for ( i = 0; i < NUM_FT_AXES; i++)
            {
                /*display the numbers*/
                ftRealReading[i] = ftCountsReading[i] / m_daftCountsPerUnit[i];

                m_lblaFTLabel1[i].setText( m_dfReading.format( ftRealReading[i] ) );
                if ( ftRealReading[i] < 0)
                    m_progaFTReadingBar1[i].setForeground( NEGATIVE_COLOR );
                else
                    m_progaFTReadingBar1[i].setForeground( POSITIVE_COLOR );
                m_progaFTReadingBar1[i].setValue( (int)Math.abs( ftRealReading[i] /
                    m_daftMaxes[i] * 100 ) );
            }
            //m_ftvc.setFTValues(ftRealReading);
        }

            private void update2()
        {
            lblStatus2.setText( "0x" + String.Format(  "%08x", (int)m_rdtPacket.getStatus() ) );
            lblRDTSeq2.setText( "" + m_rdtPacket.getRDTSequence() );
            lblFTSeq2.setText( "" + m_rdtPacket.getFTSequence() );
            int ftCountsReading[] = m_rdtPacket.getFTArray();
            double ftRealReading[] = new double[NUM_FT_AXES];
            int i;
            for ( i = 0; i < NUM_FT_AXES; i++)
            {
                /*display the numbers*/
                ftRealReading[i] = ftCountsReading[i] / m_daftCountsPerUnit[i];

                m_lblaFTLabel2[i].setText( m_dfReading.format( ftRealReading[i] ) );
                if ( ftRealReading[i] < 0)
                    m_progaFTReadingBar2[i].setForeground( NEGATIVE_COLOR );
                else
                    m_progaFTReadingBar2[i].setForeground( POSITIVE_COLOR );
                m_progaFTReadingBar2[i].setValue( (int)Math.abs( ftRealReading[i] /
                    m_daftMaxes[i] * 100 ) );
            }
            //m_ftvc.setFTValues(ftRealReading);
        }

            public void run()
            {
                if (sensorID == 1)
                    update1();

                if (sensorID == 2)
                    update2();
            }
        }
    }
    }
}